package Physics;

public class Matrix4 {
	/**
	 * initialize a 4x3 matrix
	 * the last four are always 0,0,0,1
	 */
     public float data[] = 
    	 	{1,0,0,0,
    		 0,1,0,0,
    		 0,0,1,0};

    // constructor makes an identity matrix
   public Matrix4()
   {  }

   public Matrix4( Matrix4 original )
   {
	   for (int i = 0; i < data.length; i++) 
	   {
		   data[i] = original.data[i];
	   }
   }
   
    // diagonal matrix
   public void setDiagonal(float a, float b, float c)
    {
        data[0] = a;
        data[5] = b;
        data[10] = c;
    }

    // matrix multiplication
    public Matrix4 multiplyByMatrix( Matrix4 o)
    {
        Matrix4 result = new Matrix4();
        result.data[0] = (o.data[0]*data[0]) + (o.data[4]*data[1]) + (o.data[8]*data[2]);
        result.data[4] = (o.data[0]*data[4]) + (o.data[4]*data[5]) + (o.data[8]*data[6]);
        result.data[8] = (o.data[0]*data[8]) + (o.data[4]*data[9]) + (o.data[8]*data[10]);

        result.data[1] = (o.data[1]*data[0]) + (o.data[5]*data[1]) + (o.data[9]*data[2]);
        result.data[5] = (o.data[1]*data[4]) + (o.data[5]*data[5]) + (o.data[9]*data[6]);
        result.data[9] = (o.data[1]*data[8]) + (o.data[5]*data[9]) + (o.data[9]*data[10]);

        result.data[2] = (o.data[2]*data[0]) + (o.data[6]*data[1]) + (o.data[10]*data[2]);
        result.data[6] = (o.data[2]*data[4]) + (o.data[6]*data[5]) + (o.data[10]*data[6]);
        result.data[10] = (o.data[2]*data[8]) + (o.data[6]*data[9]) + (o.data[10]*data[10]);

        result.data[3] = (o.data[3]*data[0]) + (o.data[7]*data[1]) + (o.data[11]*data[2]) + data[3];
        result.data[7] = (o.data[3]*data[4]) + (o.data[7]*data[5]) + (o.data[11]*data[6]) + data[7];
        result.data[11] = (o.data[3]*data[8]) + (o.data[7]*data[9]) + (o.data[11]*data[10]) + data[11];

        return result;
    }

    // transforms a vector by this matrix (note we assume the vector
	// has a 4th w-component of 1, so that we can multiply a Vector3
	// by a Matrix4 in the typical mathematical sense)
    public Vector3 multiplyByVector(Vector3 vector)
    {
        return new Vector3(
            vector.x * data[0] +
            vector.y * data[1] +
            vector.z * data[2] + data[3],

            vector.x * data[4] +
            vector.y * data[5] +
            vector.z * data[6] + data[7],

            vector.x * data[8] +
            vector.y * data[9] +
            vector.z * data[10] + data[11]
        );
    }

    // transforms the vector by the matrix
    public Vector3 transform(Vector3 vector)
    {
        return multiplyByVector(vector);
    }

    // get the determinant of the matrix
    public float getDeterminant()
    {
	    return data[8]*data[5]*data[2]+
	        data[4]*data[9]*data[2]+
	        data[8]*data[1]*data[6]-
	        data[0]*data[9]*data[6]-
	        data[4]*data[1]*data[10]+
	        data[0]*data[5]*data[10];
    }

    // sets this matrix to its inverse
    public void setInverse(Matrix4 m)
    {
        // Make sure the determinant is non-zero.
        float det = getDeterminant();
        if (det == 0) return;
        {
        	det = ((float)1.0)/det;
        }
        data[0] = (-m.data[9]*m.data[6]+m.data[5]*m.data[10])*det;
        data[4] = (m.data[8]*m.data[6]-m.data[4]*m.data[10])*det;
        data[8] = (-m.data[8]*m.data[5]+m.data[4]*m.data[9])*det;

        data[1] = (m.data[9]*m.data[2]-m.data[1]*m.data[10])*det;
        data[5] = (-m.data[8]*m.data[2]+m.data[0]*m.data[10])*det;
        data[9] = (m.data[8]*m.data[1]-m.data[0]*m.data[9])*det;

        data[2] = (-m.data[5]*m.data[2]+m.data[1]*m.data[6])*det;
        data[6] = (+m.data[4]*m.data[2]-m.data[0]*m.data[6])*det;
        data[10] = (-m.data[4]*m.data[1]+m.data[0]*m.data[5])*det;

        data[3] = (m.data[9]*m.data[6]*m.data[3]
                   -m.data[5]*m.data[10]*m.data[3]
                   -m.data[9]*m.data[2]*m.data[7]
                   +m.data[1]*m.data[10]*m.data[7]
                   +m.data[5]*m.data[2]*m.data[11]
                   -m.data[1]*m.data[6]*m.data[11])*det;
        data[7] = (-m.data[8]*m.data[6]*m.data[3]
                   +m.data[4]*m.data[10]*m.data[3]
                   +m.data[8]*m.data[2]*m.data[7]
                   -m.data[0]*m.data[10]*m.data[7]
                   -m.data[4]*m.data[2]*m.data[11]
                   +m.data[0]*m.data[6]*m.data[11])*det;
        data[11] =(m.data[8]*m.data[5]*m.data[3]
                   -m.data[4]*m.data[9]*m.data[3]
                   -m.data[8]*m.data[1]*m.data[7]
                   +m.data[0]*m.data[9]*m.data[7]
                   +m.data[4]*m.data[1]*m.data[11]
                   -m.data[0]*m.data[5]*m.data[11])*det;

    }

    // returns the matrix inverse
    public Matrix4 inverse()
    {
        Matrix4 result = new Matrix4();
        result.setInverse(this);
        return result;
    }

    // inverts this matrix
    public void invert()
    {
        setInverse(this);
    }

    // transform the vector by this matrix
    public Vector3 transformDirection( Vector3 vector) 
    {
        return new Vector3(
            vector.x * data[0] +
            vector.y * data[1] +
            vector.z * data[2],

            vector.x * data[4] +
            vector.y * data[5] +
            vector.z * data[6],

            vector.x * data[8] +
            vector.y * data[9] +
            vector.z * data[10]
        );
    }

    // transforms the vector by the matrix's inverse. If it contains scale
	// or shear, this function will not work.
    public Vector3 transformInverseDirection(Vector3 vector)
    {
        return new Vector3(
            vector.x * data[0] +
            vector.y * data[4] +
            vector.z * data[8],

            vector.x * data[1] +
            vector.y * data[5] +
            vector.z * data[9],

            vector.x * data[2] +
            vector.y * data[6] +
            vector.z * data[10]
        );
    }

    // as above, transforms the vector by the matrix inverse. Will not worth
	// with scale/shear matrices.
    public Vector3 transformInverse(Vector3 vector)
    {
        Vector3 tmp = new Vector3(vector);
        tmp.x -= data[3];
        tmp.y -= data[7];
        tmp.z -= data[11];
        return new Vector3(
            tmp.x * data[0] +
            tmp.y * data[4] +
            tmp.z * data[8],

            tmp.x * data[1] +
            tmp.y * data[5] +
            tmp.z * data[9],

            tmp.x * data[2] +
            tmp.y * data[6] +
            tmp.z * data[10]
        );
    }

    // get one column of the matrix
    public Vector3 getAxisVector(int i)
    {
        return new Vector3(data[i], data[i+4], data[i+8]);
    }

    // create a rotation matrix from the given quarternion
    public void setOrientationAndPos( Quaternion q,  Vector3 pos)
    {
        data[0] = 1 - (2*q.j*q.j + 2*q.k*q.k);
        data[1] = 2*q.i*q.j + 2*q.k*q.r;
        data[2] = 2*q.i*q.k - 2*q.j*q.r;
        data[3] = pos.x;

        data[4] = 2*q.i*q.j - 2*q.k*q.r;
        data[5] = 1 - (2*q.i*q.i  + 2*q.k*q.k);
        data[6] = 2*q.j*q.k + 2*q.i*q.r;
        data[7] = pos.y;

        data[8] = 2*q.i*q.k + 2*q.j*q.r;
        data[9] = 2*q.j*q.k - 2*q.i*q.r;
        data[10] = 1 - (2*q.i*q.i  + 2*q.j*q.j);
        data[11] = pos.z;
    }

    // fill data from an OpenGL array. Not used in DX.
    public void fillGLArray(float[] array)
    {
        array[0] = (float)data[0];
        array[1] = (float)data[4];
        array[2] = (float)data[8];
        array[3] = (float)0;

        array[4] = (float)data[1];
        array[5] = (float)data[5];
        array[6] = (float)data[9];
        array[7] = (float)0;

        array[8] = (float)data[2];
        array[9] = (float)data[6];
        array[10] = (float)data[10];
        array[11] = (float)0;

        array[12] = (float)data[3];
        array[13] = (float)data[7];
        array[14] = (float)data[11];
        array[15] = (float)1;
    }
	/**
	 * not sure what to do with this.
	 */
//	D3DXMATRIX ConvertToDX()
//	{
//		D3DXMATRIX w;
//
//		w._11 = (float)data[0];
//		w._12 = (float)data[4];
//		w._13 = (float)data[8];
//		w._14 = 0;
//
//		w._21 = (float)data[1];
//		w._22 = (float)data[5];
//		w._23 = (float)data[9];
//		w._24 = 0;
//
//		w._31 = (float)data[2];
//		w._32 = (float)data[6];
//		w._33 = (float)data[10];
//		w._34 = 0;
//
//		w._41 = (float)data[3];
//		w._42 = (float)data[7];
//		w._43 = (float)data[11];
//		w._44 = 1;
//
//		return w;
//	}

}
